#!/usr/bin/python

import unittest
import roslib;
roslib.load_manifest('framer')
from geometry_msgs.msg import Point
from ruleOfThirds import RuleOfThirds
from math import sqrt

class RuleOfThirdsTests(unittest.TestCase):
    def setUp(self):
        self.subject = RuleOfThirds()

    def testFindOrthogonalLine(self):
        points = [Point(1.0,5.0,0), Point(4.0, 10.0, 0)]
        self.assertEqual(
            self.subject.findOrthogonalLine(points),
            {'point': (2.5,7.5), 'slope':5.0/3.0}
        )
        points[0].x = 4
        self.subject.findOrthogonalLine(points)
        """Just need to test if the function throws"""
        self.assertTrue(True)

    def testFindEnds(self):
        points = [Point(1,5,0), Point(40,5,0), Point(4,4,0)]
        self.assertEqual(
            set(self.subject.findEnds(points)),
            set((points[0], points[1]))
        )
        self.assertEqual(
            set(self.subject.findEnds(points[0:2])),
            set(points[0:2])
        )

    def testFineClosestPointOnLine(self):
        line = {'point': (0.0,0.0), 'slope': 1.0}
        self.assertEqual(
            self.subject.closestPointOnLine(line, Point(0,0,0)),
            (0.0,0.0)
        )
        self.assertEqual(
            self.subject.closestPointOnLine(line, Point(1,0,0)),
            (0.5,0.5)
        )
        self.assertEqual(
            self.subject.closestPointOnLine(line, Point(1,1,0)),
            (1.0,1.0)
        )

    def testDistance(self):
        p1 = Point(1,2,0)
        p2 = Point(3,4,0)
        self.assertEqual(self.subject.distance(p1,p2), sqrt(8))
        p2 = Point(3,2,0)
        self.assertEqual(self.subject.distance(p1,p2), 2)

    def testFindPointOnLine(self):
        line = {'point': (0,0), 'slope': 1}
        dist = 1
        self.assertEqual(
            self.subject.findPointOnLine(line, dist),
            (1/sqrt(2), 1/sqrt(2))
        )
        line['point'] = (2,2)
        self.assertEqual(
            self.subject.findPointOnLine(line, dist),
            (1/sqrt(2)+2, 1/sqrt(2)+2)
        )
        line['slope'] = 0
        self.assertEqual(
            self.subject.findPointOnLine(line, dist),
            (3.0, 2.0)
        )

    def testFindPoint(self):
        pass


def main():
    unittest.main()

if __name__ == '__main__':
    main()
